
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mms.c
  * @author     baiyang
  * @date       2021-9-6
  * @introduction 任务管理系统，声光显示，日志记录等在此调用
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mms.h"

#include <rtthread.h>
#include <rtdevice.h>
#include <rthw.h>

#include <common/grapilot.h>
#include <common/time/gp_time.h>
#include <parameter/param.h>
#include <logger/blog_msg.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#include <board_config/borad_config.h>
/*-----------------------------------macro------------------------------------*/
#define MMS_EVENT_LOOP        (1<<0)
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static char thread_mms_stack[1024*4];
struct rt_thread thread_mms_handle;

static struct rt_timer mms_timer;
static struct rt_event mms_event;

static uitc_actuator_armed actuator_armed;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       1Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void mms_one_hz_loop()
{
    if (itc_copy_from_hub(ITC_ID(vehicle_actuator_armed), &actuator_armed) != -1
        && !actuator_armed.armed) {
        param_save_if_needed();
    }
}

/**
  * @brief       5Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void mms_fives_hz_loop()
{
    blog_write_gps();
}

/**
  * @brief       10Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void mms_ten_hz_loop()
{
    blog_write_mag();
    blog_write_pos_xy();
    blog_write_pos_ctrl_xy();
    blog_write_pos_z();
    blog_write_pos_ctrl_z();
    blog_write_vertical_ctrl();
    blog_write_loiter();
    blog_write_rc_in();
    blog_write_srv_out();
}

/**
  * @brief       25Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void mms_twenty_five_hz_loop()
{
    blog_write_imu2();
    blog_write_att();
    blog_write_att_ctrl();
    blog_write_att_pid();
}

/**
  * @brief       50Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void mms_fifty_hz_loop()
{

}

/**
  * @brief       100Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void mms_one_hundred_hz_loop()
{

}

/**
  * @brief       200Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void mms_two_hundred_hz_loop()
{

}

/**
  * @brief       200Hz循环
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        用于更新需要最先更新的数据
  */
static inline void mms_fast_loop()
{

}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static inline void mms_loop()
{
    if (!brd_is_vehicle_init_finish()) {
        return;
    }

    uint32_t NowMs = time_millis();

    TIMETAG_CHECK_EXECUTE2(fast,PERIOD_MS_200HZ,NowMs,mms_fast_loop();)

    TIMETAG_CHECK_EXECUTE2(100Hz,PERIOD_MS_100HZ,NowMs,mms_one_hundred_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(200Hz,PERIOD_MS_200HZ,NowMs,mms_two_hundred_hz_loop();)

    TIMETAG_CHECK_EXECUTE2(50Hz,PERIOD_MS_50HZ,NowMs,mms_fifty_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(25Hz,PERIOD_MS_25HZ,NowMs,mms_twenty_five_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(10Hz,PERIOD_MS_10HZ,NowMs,mms_ten_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(5Hz,PERIOD_MS_5HZ,NowMs,mms_fives_hz_loop();)
    TIMETAG_CHECK_EXECUTE2(1Hz,PERIOD_MS_1HZ,NowMs,mms_one_hz_loop();)
}

/**
  * @brief       定时器回调函数，发送定时器事件
  * @param[in]   parameter  
  * @param[out]  
  * @retval      
  * @note        
  */
static void mms_time_update(void* parameter)
{
    rt_event_send(&mms_event, MMS_EVENT_LOOP);
}

/**
  * @brief       飞行任务线程入口函数
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void mms_entry(void *parameter)
{
    rt_err_t res;
    rt_uint32_t recv_set = 0;
    rt_uint32_t wait_set = MMS_EVENT_LOOP;

    /* create event */
    res = rt_event_init(&mms_event, "mms", RT_IPC_FLAG_FIFO);
    
    /* register timer event */
    rt_timer_init(&mms_timer, "mms",
                    mms_time_update,
                    RT_NULL,
                    rt_tick_from_millisecond(1),
                    RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_HARD_TIMER);
    rt_timer_start(&mms_timer);
    
    while(1)
    {
        res = rt_event_recv(&mms_event, wait_set, RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, 
                                RT_WAITING_FOREVER, &recv_set);
        
        if(res == RT_EOK){
            if(recv_set & MMS_EVENT_LOOP){
                mms_loop();
            }
        }
    }
}

gp_err task_mms_init(void)
{
    rt_err_t res;

    res = rt_thread_init(&thread_mms_handle,
                           "mms",
                           mms_entry,
                           RT_NULL,
                           &thread_mms_stack[0],
                           sizeof(thread_mms_stack),PRIORITY_MMS,5);

    RT_ASSERT(res == RT_EOK);

    if (res == RT_EOK) {
        rt_thread_startup(&thread_mms_handle);
    }

    brd_set_vehicle_init_stage(INIT_STAGE_MMS);

    return res;
}

/*------------------------------------test------------------------------------*/


